#include "gd32w51x.h"
#include "stdio.h"
#include "DevicesDelay.h"
#include "DevicesIIC.h"
#include "DevicesBMI055.h"


void bmi055_init(void)
{
    vIIC0Init();

    bmi055WriteByte(BMI055_ACC_ADDR, ACC_SFRSET, 0xB6); //acc soft reset, check
    vDelayUs(100);
    bmi055WriteByte(BMI055_ACC_ADDR, ACC_RANGE, BMA2x2_RANGE_4G); //0x03:2g 0x05:4g 0x08:8g 0x0a:16g
    vDelayUs(10);
    bmi055WriteByte(BMI055_ACC_ADDR, ACC_BW, BMA2x2_BW_125HZ); //
    vDelayUs(10);
    // bmi055WriteByte(BMI055_ACC_ADDR, ACC_OFC_CTRL, 0x07); //enable acc slow offset compensation
    // vDelayUs(10);

    bmi055WriteByte(BMI055_GYRO_ADDR, GYRO_RANGE, BMG160_RANGE_2000); //0x00:2000 0x01:1000 0x02:500 0x03:250 0x04:125
    vDelayUs(10);
    bmi055WriteByte(BMI055_GYRO_ADDR, GYRO_BW, BMG160_BW_230_HZ); //
    vDelayUs(10);
    bmi055WriteByte(BMI055_GYRO_ADDR, GYRO_INT_EN0, 0x80); //new data int enable and auto-offset compensation
    vDelayUs(10);
    // bmi055WriteByte(BMI055_GYRO_ADDR, GYRO_SOC, 0x67); //enables gyro slow offset compensation for x/y/z
    // vDelayUs();
    // bmi055WriteByte(BMI055_GYRO_ADDR, GYRO_FOC, 0xc7); //enables fast offset compensation for x/y/z
    // vDelayUs();
    // bmi055WriteByte(BMI055_ACC_ADDR, ACC_SFRSET, 0xb6); //acc soft reset, check
}

int8_t bmi055ReadState(void)
{
    int32_t i = 0;
    uint8_t ucState = 0;

    for(i = 0; i < 5; ++i)
    {
        bmi055ReadnByte(BMI055_GYRO_ADDR, GYRO_INT_STATUS1, &ucState, 1);

        if(ucState & 0x80)
            return ucState;
    }

    return 0;
}

int8_t bmi055ReadSonsor(float *ax, float *ay, float *az,float *gx, float *gy, float *gz)
{
    uint8_t buf[6];
    int16_t gg_x = 0, gg_y = 0, gg_z = 0;
    int16_t a_x = 0, a_y = 0, a_z = 0;

    /* 读取IMU是否已经准备好数据 */
    if(bmi055ReadState() == 0)
        return -1;

    /****************陀螺仪****************************/
    bmi055ReadnByte(BMI055_GYRO_ADDR, GYRO_X_LSB, buf, 6);

    gg_x = (((uint16_t)buf[1] << 8) | buf[0]);
    gg_y = (((uint16_t)buf[3] << 8) | buf[2]);
    gg_z = (((uint16_t)buf[5] << 8) | buf[4]);

    *gx = gg_x / 32768.0f * 1000.0f;
    *gy = gg_y / 32768.0f * 1000.0f;
    *gz = gg_z / 32768.0f * 1000.0f;



    /****************加速度计****************************/
    bmi055ReadnByte(BMI055_ACC_ADDR, ACC_X_LSB, buf, 6);

    a_x = (((uint16_t)buf[1] << 8) | buf[0]);
    a_y = (((uint16_t)buf[3] << 8) | buf[2]);
    a_z = (((uint16_t)buf[5] << 8) | buf[4]);

    *ax = a_x / 32768.0f * 4.0f;
    *ay = a_y / 32768.0f * 4.0f;
    *az = a_z / 32768.0f * 4.0f;

    return 0;
}

int8_t bmi055ReadTemperature(float *pfTemperature)
{
    int8_t cTemperature = 0;

    /* 读取IMU是否已经准备好数据 */
    if(bmi055ReadState() == 0)
        return -1;

    /* 温度数据 */
    bmi055ReadnByte(BMI055_ACC_ADDR, ACC_TEMPERATURE, (uint8_t *)(&cTemperature), 1);

    /* the slope of the temperature sensor is 0.5K/LSB, its center temperature is 23% [ACC 0x08, temp = 0x00] */
    *pfTemperature = cTemperature * 0.5f + 23.0f;

    return 0;
}

int8_t bmi055ReadID(uint16_t *pusIDValue)
{
    uint8_t usIDBuff[2] = {0};

    /* 读取IMU是否已经准备好数据 */
    if(bmi055ReadState() == 0)
        return -1;

    /* ID */
    bmi055ReadnByte(BMI055_ACC_ADDR, ACC_CHIPID, &usIDBuff[0], 1);
    bmi055ReadnByte(BMI055_GYRO_ADDR, GYRO_CHIPID, &usIDBuff[1], 1);

    *pusIDValue = ((uint16_t)usIDBuff[0] << 8) + usIDBuff[1];

    return 0;
}

void bmi055WriteByte(uint8_t IDAddr, uint8_t regAddr, uint8_t byteValue)
{
    cI2CxDMAWriteDatas(I2C0, IDAddr, regAddr, &byteValue, 1);
}

void bmi055ReadnByte(uint8_t IDAddr, uint8_t regAddr, uint8_t *pBuff, uint8_t length)
{
    cI2CxDMAReadDatas(I2C0, IDAddr, regAddr, pBuff, length);
}
